#include "zf_common_headfile.h"
/* ������������ */
float Servo_out = 0;
/* ģʽѡ�񣬾��ٻ��ǵ��������ٶ���PID���� */
//#define JUNSU
/* @�������ƣ�void silun_contorl(void)
 * @���ã����ֿ��Ʒ�����
 * @���ߣ�����
 * @˵����19�쾵ͷ�����ֿ��Ʒ���
 * @��������
 * @����ֵ����
 * */
uint8 con = 0;
void silun_contorl(void)
{
    /* ���PD���� */
    Servo_out = PID_roll_Servo();
    /* ���������� */
    Servo_Out(Servo_out);
    /* �ٶȻ����� */
#ifdef JUNSU
    Speed_out_left = Speed_out_right  = silun_PID_speed_left(Encoder_RawSpeedSum);
#else
    Speed_out_left = silun_PID_speed_left(Encoder_LeftSpeed);
    Speed_out_right = silun_PID_speed_right(Encoder_RightSpeed);
#endif
    /* ת�򻷼��� */
    Turn_PID(Servo_out);
    if(blue_test==0)
    {
        /* �����ģ���� */
        Machine_LeftOut = Speed_out_left + TurnSpeed_Out;
        /* �����ģ���� */
        Machine_RightOut = Speed_out_right - TurnSpeed_Out;
    }
    else if(blue_test == 1)
    {
        Machine_LeftOut = Speed_out_left;
        Machine_RightOut = Speed_out_right;
    }
    else if(blue_test == 3)
    {
        Machine_LeftOut =Speed_Aim + TurnSpeed_Out;
        Machine_RightOut =Speed_Aim - TurnSpeed_Out;
    }
    /* ������ */
    Speed_Out(Machine_LeftOut, Machine_RightOut);
}
/********************************************��ֲ��ǰʦ�ֵĴ���******************************************************/

#if ADRC == 0
/******************************ADRC���Ʒ���*************************************************************/
//����������11��������Ҫ�û������Ĳ���
/****************TD**********/
float r = 500,                  //���ٸ�������
h = 0.002;                //�˲�����,ϵͳ���ò��� ����ſ������ڣ�

/**************ESO**********/
float b = 100,            //ϵͳϵ��
delta = 0.01,           //deltaΪfal��e��alpha��delta������������������  5h<delta<10h
belta01 = 300,            //����״̬�۲�����������1  ��Z1���������
belta02 = 500,            //����״̬�۲�����������2
belta03 = 0.00005;        //����״̬�۲�����������3

/**************NLSEF*******/
float alpha1 = 0.8,         //0-1
alpha2 = 1.2,         //����1
belta1 = 0.85,        //���������ź�����
belta2 = 1.2;         //����΢���ź�����

/*****************************fhan����*********************************/
float fhan(float x1, float x2, float r, float h)
{
/*****************************��һ��************************/
//  float d    = 0,
//        a    = 0,
//              a0   = 0,
//              a1   = 0,
//              a2   = 0,
//              y    = 0,
//              fhan = 0;
//
//  d    =  r*h*h;
//  a0   =  h*x2;
//  y    =  x1+a0;
//  a1   =  sqrtf(d*(d+8*fabsf(y)));
//  a2   =  a0 + sign(y)*(a1 - d)/2.0;
//  a    =  (a0+y)*(sign(y+d)-sign(y-d))/2.0  + a2*(1-(sign(y+d)-sign(y-d))/2.0);
//  fhan = -r*(a/d)*(sign(y+d)-sign(y-d))/2.0 - r*sign(a)*(1-(sign(a+d)-sign(a-d))/2.0);
/******************************�ڶ���**********************/
float deltaa = 0,
    deltaa0 = 0,
    y = 0,
    a0 = 0,
    a = 0,
    fhan = 0;

deltaa = r * h;
deltaa0 = deltaa * h;
y = x1 + x2 * h;
a0 = sqrtf(deltaa * deltaa + 8 * r * fabsf(y));
if (fabsf(y) <= deltaa0)
    a = x2 + y / h;
else
    a = x2 + 0.5 * (a0 - deltaa) * sign(y);
if (fabsf(a) <= deltaa)
    fhan = -r * a / deltaa;
else
    fhan = -r * sign(a);

return fhan;
}
/************************************sign����***************************/
float sign(float x)
{
    if (x > 0)
        return 1;
    else if (x < 0)
        return -1;
    else
        return 0;
}
/*******************************fal����**********************************/
float fal(float e, float alpha, float delta)
{
    float result = 0, fabsf_e = 0;

    fabsf_e = fabsf(e);

    if (delta >= fabsf_e)
        result = e / powf(delta, 1.0 - alpha);
    else
        result = powf(fabsf_e, alpha) * sign(e);

    return result;
}


//�м������������Ҫ�û������Լ���ֵ
/****************TD*******************/
float X1 = 0,//��������
X2 = 0,//���������΢��
/****************ESO******************/
e = 0,//���
z1 = 0,//���ٷ���ֵ
z2 = 0,//���ٷ���ֵ��΢��
z3 = 0,//����ϵͳ���Ŷ������Ŷ���
/**************NLSEF******************/
u = 0;//���ֵ
/********************************ADRC************************************
 * ��������ADRC_Speed()
 * �������ã�ADRC�ٶȿ��Ʒ���
 * ���������v:Ŀ���ٶȣ�y:������ֵ
 * ����ֵ�� u: PWM���
 * ���ߣ������
 * ʱ�䣺2021.05.01
 ******************************************************************/
float ADRC_Speed(float v, float y)
{
    float u0 = 0,
            e1 = 0,
            e2 = 0;
    /******************************TD****************************************/
    X1 = X1 + h * X2;
    X2 = X2 + h * fhan(X1 - v, X2, r, h);

    /******************************ESO***************************************/
    e = z1 - y;
    z1 = z1 + h * (z2 - belta01 * e);
    z2 = z2 + h * (z3 - belta02 * fal(e, 0.5, delta) + b * u);
    z3 = z3 + h * (-belta03 * fal(e, 0.25, delta));
    /******************�޷���ADRC�����Ļ�����ﵽ�޷�����********************/
    if (z1 >= 30000) z1 = 30000;
    if (z1 <= -30000) z1 = -30000;
    if (z2 >= 30000) z2 = 30000;
    if (z2 <= -30000) z2 = -30000;
    if (z3 >= 30000) z3 = 30000;
    if (z3 <= -30000) z3 = -30000;
    /******************************NLSEF*************************************/
    e1 = X1 - z1;
    e2 = X2 - z2;

    u0 = belta1 * fal(e1, alpha1, delta) + belta2 * fal(e2, alpha2, delta);   //����0<alpha1<1<alpha2
    u = u0 - z3 / b;

    return u;
}
#endif
